/*
 *  align3p.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 06/08/2012
 *
 */

#include "align3p.h"

Align3P::Align3P()
:	Constraint()
{}

// ***

Align3P::Align3P(vector<string>& mn, int di)
:	Constraint(mn,di)
{
	isQuadratic = false;
	isScalar = true;
}

// ***

void Align3P::calcPhi(const VectorXd & Q)
{
	// Q is a vector containing the cartesian coordinates of the 3 points
	// Q = (x_1, y_1, z_1, x_2, y_2, z_2, x_3, y_3, z_3)
	// This means that Q(0)=x_1, etc.	
	
	// Phi is a number
	// Uncomment the following line and write here the expression of Phi in terms of the components of Q
	Phi = sqrt(pow(((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2))),2)+pow(((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5))),2)+pow(((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))),2));	
	
}

// ***

void Align3P::calcDPhi(const VectorXd & Q)
{
	
	// dPhi is a vector
	// Uncomment the following line and write here the expression of DPhi in terms of the components of Q
	// dPhi =
    
	if(Phi != 0)
	{
	dPhi(0)=((Q(8)-Q(5))*((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5)))+(-1)*(Q(7)-Q(4))*((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))))/Phi;
	//
	dPhi(1)=((-1)*(Q(8)-Q(5))*((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2)))+(Q(6)-Q(3))*((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))))/Phi;
	//
	dPhi(2)=((Q(7)-Q(4))*((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2)))+(-1)*(Q(6)-Q(3))*((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5))))/Phi;
	//
	dPhi(3)=((-1)*(Q(8)-Q(2))*((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5)))+(Q(7)-Q(1))*((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))))/Phi;
	//
	dPhi(4)=((Q(8)-Q(2))*((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2)))+(-1)*(Q(6)-Q(0))*((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))))/Phi;
	//
	dPhi(5)=((-1)*(Q(7)-Q(1))*((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2)))+(Q(6)-Q(0))*((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5))))/Phi;
	//
	dPhi(6)=((Q(5)-Q(2))*((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5)))+(-1)*(Q(4)-Q(1))*((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))))/Phi;
	//
	dPhi(7)=((-1)*(Q(5)-Q(2))*((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2)))+(Q(3)-Q(0))*((Q(3)-Q(0))*(Q(7)-Q(4))-(Q(6)-Q(3))*(Q(4)-Q(1))))/Phi;
	//
	dPhi(8)=((Q(4)-Q(1))*((Q(4)-Q(1))*(Q(8)-Q(5))-(Q(7)-Q(4))*(Q(5)-Q(2)))+(-1)*(Q(3)-Q(0))*((Q(6)-Q(3))*(Q(5)-Q(2))-(Q(3)-Q(0))*(Q(8)-Q(5))))/Phi;
	}
     
    /*
    int j;
    VectorXd QQ = Q;
    for (j=0; j<dPhi.size(); ++j)
    {
        QQ(j) += perturbation;
        calcPhi(QQ);
    
        dPhi(j) = Phi;
        
        QQ(j)-= 2.0*perturbation;
        calcPhi(QQ);
        
        dPhi(j) -= Phi;
        
        QQ(j)+= perturbation;
    }
    dPhi /= (2.0*perturbation);
    calcPhi(QQ);
     */
	/*
	cout << "dPhi con la formula " << endl;
	cout << dPhi << endl;
	int i;
	int j;
	
	VectorXd QQ = Q;
	double per = 1.0e-7;
	
	if(Phi!=0)
	{		
		for (j=0;j< 3*dim; ++j)
		{
			QQ(j)+= per;
		
			calcPhii(QQ);
			dPhi(j) = Phi; 
			
			QQ(j)-= 2.0*per;
			calcPhii(QQ);
			dPhi(j) -= Phi; 
			dPhi(j) /= (2.0*per);
			
			QQ(j)+= per;
		}
	}
		
	cout << "dPhi aproximado " << endl;
	cout << dPhi << endl;
	// cout <<"Valores de dPhi:";
	// cout <<'\n';
	// cout << dPhi;
	// cout <<'\n';
	// cout <<'\n';
	// cout << dPhi;
	 
	*/

}

// ***

void Align3P::calcDDPhi(const VectorXd & Q)
{
	// ddPhi is a matrix
	// Uncomment the following line and write here the expression of ddPhi in terms of the components of Q
	// ddPhi =
    
    int j;
    VectorXd QQ = Q;
    
    //cout << "Perturbation = " << perturbation << endl;
    for (j=0;j<ddPhi.cols(); ++j)
    {
        QQ(j)+= perturbation;
        calcPhi(QQ);
        calcDPhi(QQ);
            
        ddPhi.col(j) = dPhi;
            
        QQ(j)-= 2.0*perturbation;
        calcPhi(QQ);
        calcDPhi(QQ);
            
        ddPhi.col(j) -= dPhi;
        
        QQ(j)+= perturbation;
    }
    ddPhi /= (2.0*perturbation);
    calcPhi(Q);
    calcDPhi(Q);
    // cout << "ddPhi = " << ddPhi << endl;
}

// ***

void Align3P::setFqSymmetries()
{}

// ***

void Align3P::setKqSymmetries()
{}

// ***

void Align3P::setParamAtCons(vector<double> & par)
{
	if (par[1]!=0.0) penalty = par[1];
}
